
#include <tuw_voronoi_graph/segment.h>
#include <limits>

namespace tuw_graph
{
uint32_t Segment::static_id_ = 0;

void Segment::cleanNeighbors(uint32_t _id)
{
    for (uint32_t i = 0; i < predecessor_.size(); i++)
    {
        if (predecessor_[i] == _id)
        {
            predecessor_.erase(predecessor_.begin() + i);
        }
    }

    for (uint32_t i = 0; i < successor_.size(); i++)
    {
        if (successor_[i] == _id)
        {
            successor_.erase(successor_.begin() + i);
        }
    }
}

void Segment::decreaseNeighborIdAbove(uint32_t _id)
{
    if (id_ >= _id) id_--;
    for (unsigned int &i : predecessor_)
    {
        if (i >= _id) i--;
    }
    for (unsigned int &i : successor_)
    {
        if (i >= _id) i--;
    }
}

void Segment::addPredecessor(const uint32_t _predecessor)
{
    predecessor_.push_back(_predecessor);
}

void Segment::addSuccessor(const uint32_t _successor)
{
    successor_.push_back(_successor);
}

Segment::Segment(const std::vector<Eigen::Vector2d> &_points, const float _min_space)
        : predecessor_(), successor_(), optimizedStart_(false), optimizedEnd_(false)
{
    if (!_points.empty())
    {
        start_ = _points.front();
        end_ = _points.back();
        length_ = _points.size();
        min_space_ = _min_space;
        wayPoints_ = _points;
    }
    id_ = static_id_++;
}

Segment::Segment(const uint32_t _id, const std::vector<Eigen::Vector2d> &_points, const float _min_space)
        : predecessor_(), successor_(), optimizedStart_(false), optimizedEnd_(false)
{
    if (!_points.empty())
    {
        start_ = _points.front();
        end_ = _points.back();
        length_ = _points.size();
        min_space_ = _min_space;
        wayPoints_ = _points;
    }
    id_ = _id;
}

void Segment::setStart(const Eigen::Vector2d &_pt)
{
    if (wayPoints_.empty()) wayPoints_.emplace_back(_pt);
    wayPoints_[0] = _pt;
    start_ = _pt;
}

void Segment::setEnd(const Eigen::Vector2d &_pt)
{
    while (wayPoints_.size() <= 1)
    {
        wayPoints_.emplace_back(_pt);
    }
    wayPoints_[wayPoints_.size() - 1] = _pt;
    end_ = _pt;
}

uint32_t Segment::getId() const
{
    return id_;
}

void Segment::setId(uint32_t _id)
{
    id_ = _id;
}

const Eigen::Vector2d &Segment::getEnd() const
{
    return end_;
}

const Eigen::Vector2d &Segment::getStart() const
{
    return start_;
}

const std::vector<uint32_t> &Segment::getPredecessors() const
{
    return predecessor_;
}

const std::vector<uint32_t> &Segment::getSuccessors() const
{
    return successor_;
}

bool Segment::containsPredecessor(const uint32_t _predecessor)
{
    for (const auto &pred : predecessor_)
    {
        if (pred == _predecessor) return true;
    }
    return false;
}


bool Segment::containsSuccessor(const uint32_t _successor)
{
    for (unsigned int i : successor_)
    {
        if (i == _successor) return true;
    }
    return false;
}

void Segment::resetId()
{
    static_id_ = 0;
}

std::vector<Eigen::Vector2d> Segment::getPath() const
{
    return wayPoints_;
}

void Segment::setPath(const std::vector<Eigen::Vector2d> &_points)
{
    if (!_points.empty())
    {
        start_ = _points.front();
        end_ = _points.back();
        length_ = _points.size();
        wayPoints_ = _points;
    }
}

float Segment::getMinPathSpace() const
{
    return min_space_;
}

void Segment::setMinPathSpace(const float _space)
{
    min_space_ = _space;
}

int Segment::getLength() const
{
    return length_;
}

bool &Segment::getOptStart()
{
    return optimizedStart_;
}

bool &Segment::getOptEnd()
{
    return optimizedEnd_;
}
}
